/*
 * cpu.c
 *
 *  Created on: Feb 18, 2013
 *      Author: ismith
 */

#include "cpu.h"

typedef struct
{
	RegisterStr raw;
	Byte opcode_class;
	Byte subopcode;
} Instruction;

RegisterStr read_byte(CpuPtr cpu, RegisterStr address)
{
	Memory_setMemoryAddressRegister(cpu->memory, address);
	Memory_performRead(cpu->memory);
	RegisterStr value = Memory_getMemoryDataRegister(cpu->memory);

	return value;
}

RegisterStr read_word(CpuPtr cpu, RegisterStr address)
{
	RegisterStr value = read_byte(cpu, address) << 8;
	address++;
	value |= read_byte(cpu, address);

	return value;
}

void write_byte(CpuPtr cpu, RegisterStr address, Byte value)
{
	Memory_setMemoryAddressRegister(cpu->memory, address);
	Memory_setMemoryDataRegister(cpu->memory, value);
	Memory_performWrite(cpu->memory);
}

void write_word(CpuPtr cpu, RegisterStr address, RegisterStr value)
{
	Byte byte_value = (value & 0xff00) >> 8;
	write_byte(cpu, address, byte_value);

	address++;
	byte_value = value & 0xff;
	write_byte(cpu, address, byte_value);
}

RegisterStr sign_extend(RegisterStr source, int start_bit, int end_bit)
{
	Bit sign_bit = (source & (1 << start_bit)) >> start_bit;
	unsigned short value_mask = 0;
	int i;

	for (i = end_bit; i < start_bit; i++)
		value_mask |= 1 << i;

	source = (source & value_mask) >> end_bit;

	if (sign_bit)
		for (i = start_bit - end_bit; i < 16; i++)
			source |= 1 << i;

	return source;
}

RegisterStr get_address(
		CpuPtr cpu,
		int other_register_index,
		Byte modifier,
		int for_write,
		int bytes
)
{
	RegisterStr address;
	int index;

	switch (modifier)
	{
		case 0: // Direct mode: M[$Ri]
			address = RegisterFile_getRegisterWord(
					cpu->reg_file,
					other_register_index
			);
			break;

		case 1: // Base+offset mode: M[BP + $Ri]
			address = RegisterFile_getRegisterWord(
					cpu->reg_file,
					BP_REG_INDEX
			);
			address += RegisterFile_getRegisterWord(
					cpu->reg_file,
					other_register_index
			);
			break;

		case 2: // Index mode: M[IP-0] or MP[IP-1]
			index = (for_write ? IP1_REG_INDEX : IP0_REG_INDEX);
			address = RegisterFile_getRegisterWord(cpu->reg_file, index);
			RegisterFile_setRegisterWord(cpu->reg_file, index, address + bytes);
			break;

		case 3: // Pointer mode: M[M[$Ri]]
			address = RegisterFile_getRegisterWord(
					cpu->reg_file,
					other_register_index
			);
			address = read_word(cpu, address);
			break;

		default:
			address = 0;
			break;
	}

	return address;
}

void fetch(CpuPtr cpu)
{
	RegisterStr address = Register_getRegister(cpu->pc);
	RegisterStr instruction = read_word(cpu, address);

	address += 2;
	Register_setRegister(cpu->pc, address);
	Register_setRegister(cpu->ir, instruction);
}

void push_pop(CpuPtr cpu, int register_index, Byte modifier)
{
	RegisterStr address = RegisterFile_getRegisterWord(
			cpu->reg_file,
			SP_REG_INDEX
	);
	RegisterStr value;

	switch (modifier)
	{
		case 1: // PUSHB
			value = RegisterFile_getRegisterWord(cpu->reg_file, register_index);
			write_byte(cpu, address, value);
			address--;
			break;

		case 2: // POPB
			address++;
			value = read_byte(cpu, address);
			RegisterFile_setRegisterLOB(cpu->reg_file, register_index, value);
			break;

		case 3: // PUSHW
			address--;
			value = RegisterFile_getRegisterWord(cpu->reg_file, register_index);
			write_word(cpu, address, value);
			address--;
			break;

		case 4: // POPW
			address++;
			value = read_word(cpu, address);
			RegisterFile_setRegisterWord(cpu->reg_file, register_index, value);
			address++;
			break;
	}

	RegisterFile_setRegisterWord(cpu->reg_file, SP_REG_INDEX, address);
}


void loadstore(CpuPtr cpu, Instruction instruction)
{
	int register_index = (instruction.raw & 0x0780) >> 7;
	int other_index = (instruction.raw & 0x0078) >> 3;
	Byte modifier = instruction.raw & 0x0007;
	RegisterStr address;
	RegisterStr value;

	switch (instruction.subopcode)
	{
		case 1: // LDI (load immediate)
			value = sign_extend(instruction.raw, 6, 0);
			RegisterFile_setRegisterWord(cpu->reg_file, register_index, value);
			break;

		case 2: // LDA (load address)
			address = Register_getRegister(cpu->pc);
			value = read_word(cpu, address);

			address += 2;
			Register_setRegister(cpu->pc, address);

			RegisterFile_setRegisterWord(cpu->reg_file, register_index, value);
			break;

		case 3: // LDB (load byte)
			address = get_address(cpu, other_index, modifier, 0, 1);
			value = read_byte(cpu, address);
			RegisterFile_setRegisterLOB(cpu->reg_file, register_index, value);
			break;

		case 4: // LDW (load word)
			if (modifier == 4) // MOV instruction
				value = RegisterFile_getRegisterWord(
						cpu->reg_file,
						other_index
				);
			else // Standard LDW instruction
			{
				address = get_address(cpu, other_index, modifier, 0, 2);
				value = read_word(cpu, address);
			}

			RegisterFile_setRegisterWord(cpu->reg_file, register_index, value);
			break;

		case 5: // STB (store byte)
			address = get_address(cpu, other_index, modifier, 1, 1);
			value = RegisterFile_getRegisterLOB(cpu->reg_file, register_index);
			write_byte(cpu, address, value);
			break;

		case 6: // STW (store word)
			address = get_address(cpu, other_index, modifier, 1, 2);
			value = RegisterFile_getRegisterWord(cpu->reg_file, register_index);
			write_word(cpu, address, value);
			break;

		case 7: // Stack instructions.
			push_pop(cpu, register_index, modifier);
			break;
	}
}

void alu_op(CpuPtr cpu, int register_index, int other_index, Byte modifier)
{
	RegisterStr value1 = RegisterFile_getRegisterWord(
			cpu->reg_file,
			register_index
	);
	RegisterStr value2 = RegisterFile_getRegisterWord(
			cpu->reg_file,
			other_index
	);

	alu_set_operand_a(cpu->alu, value1);
	alu_set_operand_b(cpu->alu, value2);

	switch (modifier)
	{
		case 0: // ADD
			alu_add(cpu->alu);
			break;

		case 1: // SUB
			alu_sub(cpu->alu);
			break;

		case 2: // MUL
			alu_mul(cpu->alu);
			break;

		case 3: // DIV
			alu_div(cpu->alu);
			value1 = alu_get_result_r2(cpu->alu);
			RegisterFile_setRegisterWord(cpu->reg_file, 8, value1);
			break;

		case 4: // AND
			alu_and(cpu->alu);
			break;

		case 5: // OR
			alu_or(cpu->alu);
			break;

		case 6: // XOR
			alu_xor(cpu->alu);
			break;

		case 7: // NOT
			alu_not(cpu->alu);
			break;
	}

	value1 = alu_get_result_r(cpu->alu);
	RegisterFile_setRegisterWord(cpu->reg_file, 0, value1);


	RegisterStr status_value = Register_getRegister(cpu->sw) & 0x0fff;
	status_value |= alu_get_status_n(cpu->alu) << N_BIT_POS;
	status_value |= alu_get_status_z(cpu->alu) << Z_BIT_POS;
	status_value |= alu_get_status_c(cpu->alu) << C_BIT_POS;
	status_value |= alu_get_status_o(cpu->alu) << O_BIT_POS;
	Register_setRegister(cpu->sw, status_value);
}

void alu(CpuPtr cpu, Instruction instruction)
{
	int register_index = (instruction.raw & 0x0780) >> 7;
	int other_index = (instruction.raw & 0x0078) >> 3;
	Byte modifier = instruction.raw & 0x0007;
	RegisterStr value;

	switch (instruction.subopcode) {
		case 0: // SHL (shift-left)
			value = RegisterFile_getRegisterWord(cpu->reg_file, register_index);
			value <<= 1;
			RegisterFile_setRegisterWord(cpu->reg_file, 0, value);
			break;

		case 1: // Standard ALU instruction
			alu_op(cpu, register_index, other_index, modifier);
			break;

		case 2: // SHR (shift-right)
			value = RegisterFile_getRegisterWord(cpu->reg_file, register_index);
			value >>= 1;
			RegisterFile_setRegisterWord(cpu->reg_file, 0, value);
			break;
	}
}

int should_branch(CpuPtr cpu, Byte modifier)
{
	RegisterStr sw = Register_getRegister(cpu->sw);

	switch (modifier)
	{
		case 0: // BRn
			return sw & (1 << N_BIT_POS);

		case 1: // BRz
			return sw & (1 << Z_BIT_POS);

		case 2: // BRc
			return sw & (1 << C_BIT_POS);

		case 3: // BRo
			return sw & (1 << O_BIT_POS);

		default:
			return FALSE;
	}
}

void control(CpuPtr cpu, Instruction instruction)
{
	int register_index = (instruction.raw & 0x0780) >> 7;
	Byte modifier = instruction.raw & 0x0003;
	RegisterStr address = RegisterFile_getRegisterWord(cpu->reg_file, SP_REG_INDEX);

	switch (instruction.subopcode)
	{
		case 0: //BR      immed11         3    N/A     // PC <- (PC) + SEXT(immed11) - local branch
			cpu_set_pc(cpu, cpu_get_pc(cpu) + sign_extend(instruction.raw, 10, 0));
			break;

		case 1: //BR	     $Rr  (immed7=0) 1    N/A     // PC <- ($Rr)
			cpu_set_pc(cpu, RegisterFile_getRegisterWord(cpu->reg_file, register_index));
			break;

		case 2: //BRc     immed9          4
				//BRn                           00     // PC <- PC + SEXT(immed9) if N=1
				//BRz                           01     // PC <- PC + SEXT(immed9) if Z=1
				//BRc                           10     // PC <- PC + SEXT(immed9) if C=1
				//BRo                           11     // PC <- PC + SEXT(immed9) if O=1
			if (should_branch(cpu, modifier))
				cpu_set_pc(cpu, cpu_get_pc(cpu) + sign_extend(instruction.raw, 10, 2));
			break;

		case 3:  //BRc     $Rr             5
				 //BRn                           00     // PC <- PC + SEXT(immed9) if N=1
				 //BRz                           01     // PC <- PC + SEXT(immed9) if Z=1
				 //BRc                           10     // PC <- PC + SEXT(immed9) if C=1
				 //BRo                           11     // PC <- PC + SEXT(immed9) if O=1
			if (should_branch(cpu, modifier))
				cpu_set_pc(cpu, RegisterFile_getRegisterWord(cpu->reg_file, register_index));
			break;

		case 4: // JSR     $Rr (immed7=0)  1    N/A      PUSHW(PC); PC <- ($Rr)
			address--;
			write_word(cpu, address, cpu_get_pc(cpu));
			address--;
			RegisterFile_setRegisterWord(cpu->reg_file, SP_REG_INDEX, address);
			//done pushing PC on and updating SP register, post incremented push.
			cpu_set_pc(cpu, RegisterFile_getRegisterWord(cpu->reg_file, register_index));
			break;

		case 5:  //JSRc    $Rr             5
				 //JSRn                          00     // PUSHW(PC); PC <- ($Rr) if N=1
				 //JSRz                          01     // PUSHW(PC); PC <- ($Rr) if Z=1
				 //JSRc                          10     // PUSHW(PC); PC <- ($Rr) if C=1
				 //JSRo                          11     // PUSHW(PC); PC <- ($Rr) if O=1
			if (should_branch(cpu, modifier))
			{
				address--;
				write_word(cpu, address, cpu_get_pc(cpu));
				address--;
				RegisterFile_setRegisterWord(cpu->reg_file, SP_REG_INDEX, address);
				//done pushing PC on and updating SP register, post incremented push.
				cpu_set_pc(cpu, RegisterFile_getRegisterWord(cpu->reg_file, register_index));
			}
			break;

		case 6: // TRAP    vect6           6     N/A    // PUSHW(PC); PUSHW(SW); PC <- (IVT) + SEXT(vect6)
			address--;
			write_word(cpu, address, cpu_get_pc(cpu));
			address -= 2;
			write_word(cpu, address, cpu_get_sw(cpu));
			address--;
			RegisterFile_setRegisterWord(cpu->reg_file, SP_REG_INDEX, address);

			cpu_set_pc(cpu, IVT_START + sign_extend(instruction.raw, 10, 0));

			break;

		case 7: //  RET     (immed11=0)     3     N/A    // PC <- POPW
				//  RETI                    7      1     // SW <- POPW; PC <- POPW
			if (modifier == 0) //RET
			{
				address++;
				cpu_set_pc(cpu, read_word(cpu, address));
				address++;
				RegisterFile_setRegisterWord(cpu->reg_file, SP_REG_INDEX, address);
			}
			else if (modifier == 1) //RETI
			{
				address++;
				Register_setRegister(cpu->sw, read_word(cpu, address));
				address += 2;
				cpu_set_pc(cpu, read_word(cpu, address));
				address++;
				RegisterFile_setRegisterWord(cpu->reg_file, SP_REG_INDEX, address);
			}
			break;
	}
}

void misc(CpuPtr cpu, Instruction instruction)
{
	Byte port;
	int modifier = instruction.raw & 0x0001;
	int register_index;
	RegisterStr irqs;
	RegisterStr value;

	switch (instruction.subopcode)
	{
		case 0: // IN/OUT
			register_index = (instruction.raw & 0x0780) >> 7;
			port = (instruction.raw & 0x007E) >> 1;

			if (modifier == 0) // IN
			{
				value = ioctl_read(cpu->ioctl, port);
				RegisterFile_setRegisterWord(cpu->reg_file, register_index, value);
			}
			else if (modifier == 1) // OUT
			{
				value = RegisterFile_getRegisterWord(cpu->reg_file, register_index);
				ioctl_write(cpu->ioctl, port, value);
			}

			break;

		case 1: // EI/DI
			irqs = instruction.raw & 0x07f8 >> 3;

			if (modifier == 0) // EI
				value = Register_getRegister(cpu->sw) | irqs;
			else if (modifier == 1) // DI
				value = Register_getRegister(cpu->sw) & irqs;

			Register_setRegister(cpu->sw, value);

			break;

		case 2: // HALT
			cpu->stop = 1;
			break;

		case 3: // NOP
			// Do nothing...my favorite opcode.
			break;

		case 4: // LDOS
			value = sign_extend(instruction.raw, 11, 0);
			Register_setRegister(cpu->osb, cpu_get_pc(cpu) + value);
			break;

		case 5: // LDRO
			value = sign_extend(instruction.raw, 11, 0);
			Register_setRegister(cpu->txb, cpu_get_pc(cpu) + value);
			break;
	}
}

Instruction decode(CpuPtr cpu)
{
	Instruction instruction;
	instruction.raw = Register_getRegister(cpu->ir);
	instruction.opcode_class = (instruction.raw & 0xc000) >> 14;
	instruction.subopcode = (instruction.raw & 0x3800) >> 11;

	return instruction;
}

void execute(CpuPtr cpu, Instruction instruction)
{
	switch (instruction.opcode_class)
	{
		case 0:
			loadstore(cpu, instruction);
			break;

		case 1:
			alu(cpu, instruction);
			break;

		case 2:
			control(cpu, instruction);
			break;

		case 3:
			misc(cpu, instruction);
			break;
	}
}

CpuPtr cpu_new_cpu(MemoryPtr memory)
{
	CpuPtr cpu = (CpuPtr)malloc(sizeof(CpuStr));

	cpu->alu = alu_new_alu();
	cpu->ioctl = ioctl_new();
	cpu->memory = memory;
	cpu->reg_file = RegisterFile_newRegisterFile();

	cpu->pc = Register_newRegister();
	cpu->ir = Register_newRegister();
	cpu->sw = Register_newRegister();
	cpu->osb = Register_newRegister();
	cpu->txb = Register_newRegister();

	return cpu;
}

void cpu_free(CpuPtr cpu)
{
	alu_free(cpu->alu);
	ioctl_free(cpu->ioctl);
	RegisterFile_free(cpu->reg_file);

	Register_deleteRegister(cpu->pc);
	Register_deleteRegister(cpu->ir);
	Register_deleteRegister(cpu->sw);
	Register_deleteRegister(cpu->osb);
	Register_deleteRegister(cpu->txb);

	free(cpu);
}

void cpu_initialize(CpuPtr cpu)
{
	ioctl_initialize(cpu->ioctl);
	Memory_initialize(cpu->memory);
	RegisterFile_initialize(cpu->reg_file);

	Register_initialize(cpu->pc);
	Register_initialize(cpu->ir);
	Register_initialize(cpu->sw);
	Register_initialize(cpu->osb);
	Register_initialize(cpu->txb);
}

void cpu_set_pc(CpuPtr cpu, RegisterStr new_pc)
{
	Register_setRegister(cpu->pc, new_pc);
}

void cpu_step(CpuPtr cpu)
{
	if (cpu->stop)
		return;

	Instruction instruction;

	fetch(cpu);
	instruction = decode(cpu);
	execute(cpu, instruction);
}

RegisterStr cpu_get_pc(CpuPtr cpu)
{
	return Register_getRegister(cpu->pc);
}

RegisterStr cpu_get_ir(CpuPtr cpu)
{
	return Register_getRegister(cpu->ir);
}

RegisterStr cpu_get_sw(CpuPtr cpu)
{
	return Register_getRegister(cpu->sw);
}

Bit cpu_get_stop_bit(CpuPtr cpu)
{
	return cpu->stop;
}

void cpu_reset_stop_Bit(CpuPtr cpu)
{
	cpu->stop = 0x0;
}
